﻿using System;

namespace PengSW.MathHelper
{
    public static class TriEquationHelper
    {
        public static bool IsZero(double x) => x >= -double.Epsilon && x <= double.Epsilon;

        public static void Swap<T>(ref T a, ref T b)
        {
            (b, a) = (a, b);
        }

        public static void SwapRow<T>(T[,] M, int r1, int r2)
        {
            for (int i = 0; i< M.GetLength(1); i++)
            {
                Swap(ref M[r1, i], ref M[r2, i]);
            }
        }

        public static void RowDiv(double[,] M, int r, double x)
        {
            for (int i = 0; i < M.GetLength(1); i++)
            {
                M[r, i] /= x;
            }
        }

        public static void RowAdd(double[,] M, int targetRow, int sourceRow, double x)
        {
            for (int i = 0; i<M.GetLength(1); i++)
            {
                M[targetRow, i] += M[sourceRow, i] * x;
            }
        }

        public static Tuple<double, double, double> SolveTriEquation(double a1, double b1, double c1, double a2, double b2, double c2, double a3, double b3, double c3)
        {
            double[,] M = new double[3, 4] { { a1, b1, 1, c1 }, { a2, b2, 1, c2 }, { a3, b3, 1, c3 } };
            int[] aIndex = new int[] { 0, 1, 2 };

            // 将首列中的非零元通过行交换换到[0, 0]
            if (IsZero(M[0, 0]))
            {
                if (IsZero(M[1, 0]))
                {
                    if (IsZero(M[2, 0])) throw new ApplicationException("Row0 is zero row.");
                    SwapRow(M, 0, 2);
                    Swap(ref aIndex[0], ref aIndex[2]);
                }
                else
                {
                    SwapRow(M, 0, 1);
                    Swap(ref aIndex[0], ref aIndex[1]);
                }
            }

            // 将[0, 0]置1
            RowDiv(M, 0, M[0, 0]);

            // 用[0, 0]对[1, 0], [2, 0]消元
            RowAdd(M, 1, 0, -M[1, 0]);
            RowAdd(M, 2, 0, -M[2, 0]);

            // 将[1, 1], [2, 1]中的非零元行交换到[1, 1]
            if (IsZero(M[1,1]))
            {
                if (IsZero(M[2, 1])) throw new ApplicationException("(2, 1) is zero.");
                SwapRow(M, 1, 2);
                Swap(ref aIndex[1], ref aIndex[2]);
            }

            // 将[1, 1]置1
            RowDiv(M, 1, M[1, 1]);

            // 用[1, 1]对[0, 1], [2, 1]消元
            RowAdd(M, 0, 1, -M[0, 1]);
            RowAdd(M, 2, 1, -M[2, 1]);

            if (IsZero(M[2, 2])) throw new ApplicationException("(2, 2) is zero.");

            // 将[2, 2]置1
            RowDiv(M, 2, M[2, 2]);

            // 用[2, 2]对[0, 2], [1, 2]消元
            RowAdd(M, 0, 2, -M[0, 2]);
            RowAdd(M, 1, 2, -M[1, 2]);

            return new Tuple<double, double, double>(M[aIndex[0], 3], M[aIndex[1], 3], M[aIndex[2], 3]);
        }
    }
}
